/**
  ***********************************************************************************
  * @file   : Service_Communication.cpp
  * @brief  : Communication support file.This file provides access ports to interface
  *           with connected devices.
  ***********************************************************************************
                                 ##### Port List #####
  =================================================================================
  |Port Name     Physical-Layer     Data-Link-Layer    Application-Layer    Number
  |————————————————————————————————————————
  |EXAMPLE_Port       CAN1               CAN                CUSTOM            0
  |CAN2_Port          CAN2               CAN                Custom            1
  |EBUG_Port         USART1             Custom              Custom            2
  |USART2_Port       USART2              DBUS               DJI-DR16          3
  *
**/
/* Includes ------------------------------------------------------------------*/
#include "Service_Communication.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include "motor_command.h"
/* Private define ------------------------------------------------------------*/


void Task_SendADC(void *arg);
/**
* @brief  Initialization of communication service
* @param  None.
* @return None.
*/
void Service_Communication_Init(void)
{ 
  //xTaskCreate(Task_SendADC,"Com.Usart TxPort" , Tiny_Stack_Size,    NULL, PriorityHigh,   &UartTransmitPort_Handle);
}

/*---------------------------------------------- USART Manager --------------------------------------------*/
/*Task Define ---------------------------*/
TaskHandle_t UartTransmitPort_Handle;

/*Function Prototypes--------------------*/
/**
* @brief  Tasks for USART Management.
          Attention:In this version we passing the pointer of data not copying
          data itself and we only have one buff zone, so user need to process 
          the data as soon as possible in case of over-write.
* @param  None.
* @return None.
*/
void Task_SendADC (void *arg)
{
  /* Cache for Task */
  /* Pre-Load for task */
  /* Infinite loop */
  for(;;)
  {
    /* Usart Receive Port*/
//    HAL_UART_Transmit_DMA(&huart1, _PackToPCUnion.UsartData, sizeof(_PackToPCUnion.PackToPC));
	vTaskDelay(2 / portTICK_RATE_MS);
  }
}

/**
* @brief  Callback function in USART Interrupt
* @param  None.
* @return None.
*/

uint32_t User_UART1_RxCpltCallback(uint8_t* Recv_Data, uint16_t ReceiveLen)
{
	static USART_COB Usart_RxCOB;
	if (USART_RxPort!=NULL){
		Usart_RxCOB.port_num=1;
		Usart_RxCOB.len=ReceiveLen;
		Usart_RxCOB.address=Recv_Data;
		xQueueSendFromISR(USART_RxPort,&Usart_RxCOB,0);
	}
	return 0;
}

void Usart_Tx_Pack(QueueHandle_t Queue_Usart_TxPort,uint8_t port_num,uint16_t len,void* address)
{
	USART_COB Usart_TxCOB;
	Usart_TxCOB.port_num=port_num;
	Usart_TxCOB.address=address;
	Usart_TxCOB.len=len;
	xQueueSend(Queue_Usart_TxPort,&Usart_TxCOB,0);
}

static void Convert_Data(CAN_RxMessage* input, CAN_COB* output)
{
	if (input->header.StdId==0x00) output->ID=input->data[0];
	else output->ID = input->header.StdId;
  output->DLC = input->header.DLC;
	memcpy(output->Data, input->data, output->DLC);
}

void User_CAN1_RxCpltCallback(CAN_RxBuffer *CAN_RxMessage)
{
  static CAN_COB   CAN_RxCOB;
  Convert_Data(CAN_RxMessage,&CAN_RxCOB);
  //Send To CAN Receive Queue
//  if(((CAN_RxMessage->header.StdId)&0x200) == 0x200)
//	{
//		/* Calculate motor ID,and get the data of motor */
//	}
  if(CAN_RxCOB.Data[CAN_RxCOB.DLC-1] == CRC_cal(CAN_RxCOB.ID,CAN_RxCOB.Data,CAN_RxCOB.DLC) && 
        CAN_RxCOB.Data[0] == 0x32)
  {
    switch(CAN_RxCOB.ID)
    {
        case 0x01:
        {
            MOTOR1_Speed =  ((int16_t)CAN_RxCOB.Data[1] << 8) | CAN_RxCOB.Data[2];
            break;
        }
        
        case 0x02:
        {
            MOTOR2_Speed =  ((int16_t)CAN_RxCOB.Data[1] << 8) | CAN_RxCOB.Data[2];
            break;
        }
        
        case 0x03:
        {
            MOTOR3_Speed =  ((int16_t)CAN_RxCOB.Data[1] << 8) | CAN_RxCOB.Data[2];
            break;
        }
        default:
        {
            break;
        }
    }
  }

}

void User_CAN2_RxCpltCallback(CAN_RxBuffer *CAN_RxMessage)
{
  static CAN_COB   CAN_RxCOB;
  Convert_Data(CAN_RxMessage,&CAN_RxCOB);
  //Send To CAN Receive Queue
    if(CAN_RxCOB.Data[CAN_RxCOB.DLC-1] == CRC_cal(CAN_RxCOB.ID,CAN_RxCOB.Data,CAN_RxCOB.DLC) && 
        CAN_RxCOB.Data[0] == 0x32)
  {
    switch(CAN_RxCOB.ID)
    {        
        case 0x04:
        {
            MOTOR4_Speed =  ((int16_t)CAN_RxCOB.Data[1] << 8) | CAN_RxCOB.Data[2];
            break;
        }
        default:
        {
            break;
        }
    }
  }

//  if (CAN_RxCOB.ID==0x205 && RMMotor_QueueHandle != NULL){
//  }
}

//void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
//{
//	_PackToPCUnion.PackToPC.ad_data = ad_value;
//}
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
